/*
 * ICM20602陀螺仪模块通过I2c方式控制
 */
#include "head\ICM20602_I2c.h"
#include "head\MCU.h"

void ICM_IIC_Init()
{
    /*
     * 配置IIC复用引脚
     */
    GPIO_setAsPeripheralModuleFunctionInputPin(
        ICM_IIC_PORT,
        ICM_IIC_PIN_SDA + ICM_IIC_PIN_SCL
        );

    /*
     * 初始化IIC主模式
     */
    USCI_B_I2C_initMasterParam param = {0};
    param.selectClockSource = USCI_B_I2C_CLOCKSOURCE_SMCLK;
    param.i2cClk = UCS_getSMCLK(); //返回一个u32，此时SMCLK的时钟频率
    param.dataRate = USCI_B_I2C_SET_DATA_RATE_400KBPS; //选择传输速度，有100和400两种选择
    USCI_B_I2C_initMaster(ICM_IIC_BASE, &param);

    /*
     * 指定从机地址
     */
    USCI_B_I2C_setSlaveAddress(ICM_IIC_BASE,ICM20602_ADDR);

    USCI_B_I2C_enable(ICM_IIC_BASE);
    ICM20602_Init();
}

void ICM_Write_Registers(uint8_t reg, uint8_t data)
{
    uint8_t Timeing = 6;
    //Enable transmit Interrupt
    USCI_B_I2C_clearInterrupt(ICM_IIC_BASE,
        USCI_B_I2C_TRANSMIT_INTERRUPT | USCI_B_I2C_RECEIVE_INTERRUPT);//清除中断标志，及 UCBxIFG 置0
    USCI_B_I2C_enableInterrupt(ICM_IIC_BASE,
        USCI_B_I2C_TRANSMIT_INTERRUPT | USCI_B_I2C_RECEIVE_INTERRUPT);//设置中断使能，及 UCBxIE 置1

    //Delay between each transaction
    __delay_cycles(50);

    IcmTXByteCtrTotal = 2-1;                 //-1表示不发送结束信号

    IcmTxDataBuff[0] = reg;               //将要发送的数据1，寄存器地址
    IcmTxDataBuff[1] = data;              //将要发送的数据2，寄存器数值
    PIcmTxData = (uint8_t *)IcmTxDataBuff;//指向将要发送的数据的开头

    IcmRXByteCtr = 0;                         //将要接收的数据长度
//    PIcmRxData = (uint8_t *)buf;          //指向存放将要接收的数据的地址开头

    USCI_B_I2C_masterSendMultiByteStart(ICM_IIC_BASE,*PIcmTxData++);//发送S、AD+W与第一个字节（陀螺仪的寄存器地址）
    //等待停止信号发送完毕
    do{
        __delay_cycles(MSP430_FREQUENCY/2);
        Timeing--;
        if(Timeing == 0)
        {
            __no_operation();
        }
    }
    while(IcmTXByteCtrTotal != -1);
}

void ICM_Read_Len(uint8_t reg, uint8_t* buf, uint8_t len)
{
    uint8_t Timeing = 6;
    //Enable transmit Interrupt
    USCI_B_I2C_clearInterrupt(ICM_IIC_BASE,
        USCI_B_I2C_TRANSMIT_INTERRUPT | USCI_B_I2C_RECEIVE_INTERRUPT);//清除中断标志，及 UCBxIFG 置0
    USCI_B_I2C_enableInterrupt(ICM_IIC_BASE,
        USCI_B_I2C_TRANSMIT_INTERRUPT | USCI_B_I2C_RECEIVE_INTERRUPT);//设置中断使能，及 UCBxIE 置1

    //Delay between each transaction
    __delay_cycles(50);

    IcmTXByteCtrTotal = -2;                 //-2表示不发送结束信号，直接进入接收模式

    IcmTxDataBuff[0] = reg;               //将要发送的数据1，寄存器地址
    PIcmTxData = (uint8_t *)IcmTxDataBuff;//指向将要发送的数据的开头

    IcmRXByteCtr = len;                   //将要接收的数据长度
    PIcmRxData = (uint8_t *)buf;          //指向存放将要接收的数据的地址开头

    //Initiate start and send first character
    USCI_B_I2C_masterSendMultiByteStart(ICM_IIC_BASE,*PIcmTxData);

    do{
        __delay_cycles(MSP430_FREQUENCY/2);
        Timeing--;
        if(Timeing == 0)
        {
            __no_operation();
        }
    }
    while(IcmRXByteCtr!=0);
    __no_operation();
}

/* ------------- ICM20602 六轴传感器初始化 ---------------- */
uint8_t ICM20602_Init(void)
{
//    uint8_t *str;
//    str = "DBG复位ICM\n";
//    for(;*str != '\0';str++)
//    {
//        USCI_A_UART_transmitData(USCI_A3_BASE,*str);
//    }
    /* (2)复位ICM20602 */
    ICM_Write_Registers(ICM20602_PWR_MGMT1_REG, 0x80);
    __delay_cycles(2516582/10); // 利用定时器配置延迟100ms//25165824hz
//    str = "DBG唤醒ICM\n";
//    for(;*str != '\0';str++)
//    {
//        USCI_A_UART_transmitData(USCI_A3_BASE,*str);
//    }
    ICM_Write_Registers(ICM20602_PWR_MGMT1_REG, 0x00); //唤醒ICM20602
    
//    str = "DBG复位重置各个寄存器ICM\n";
//    for(;*str != '\0';str++)
//    {
//        USCI_A_UART_transmitData(USCI_A3_BASE,*str);
//    }
    /* 复位重置传感器各个通道的寄存器 */
    ICM_Write_Registers(ICM20602_USER_CTRL, 0x01);
//    __delay_cycles(2516582/10);
//    str = "DBG复位重置各个寄存器ICM2\n";
//    for(;*str != '\0';str++)
//    {
//        USCI_A_UART_transmitData(USCI_A3_BASE,*str);
//    }
    ICM_Write_Registers(ICM20602_USER_CTRL, 0x00);

//    str = "DBG设置ICM量程\n";
//    for(;*str != '\0';str++)
//    {
//        USCI_A_UART_transmitData(USCI_A3_BASE,*str);
//    }
    /* (3)配置陀螺仪满量程范围：-2000dps ~ +2000dps；加速度计满量程范围：-2g ~ +2g */
    ICM_Set_Gyro_Fsr(0x3);
    ICM_Set_Accel_Fsr(0);

    /* (4)设置采样频率50HZ */
    ICM_Set_Rate(50);

    /* (5)禁止中断，关闭硬件 */
    ICM_Write_Registers(ICM20602_INT_ENABLE_REG, 0x00);  // 关闭所有中断
    ICM_Write_Registers(ICM20602_FIFO_EN, 0x00);         // 关闭陀螺仪和加速度计的FIFO数据缓冲区
    ICM_Write_Registers(ICM20602_INT_PIN_CFG, 0x80);     // 设置INT引脚为低电平有效，开漏输出

    /* 设置时钟源 */
    ICM_Write_Registers(ICM20602_PWR_MGMT1_REG, 0x01);

    /* 设置accelerate阈值 */
    ICM_Write_Registers(ICM20602_LACCEL_WOM_X_THR, 0x7F);
    ICM_Write_Registers(ICM20602_LACCEL_WOM_Y_THR, 0x7F);
    ICM_Write_Registers(ICM20602_LACCEL_WOM_Z_THR, 0x7F);

    /* 开启加速度计的阈值中断 */
    ICM_Write_Registers(ICM20602_ACCEL_INTEL_CTRL, 0x80);
    __delay_cycles(2516582/100);

    /* 使能中断 */
    ICM_Write_Registers(ICM20602_INT_ENABLE_REG, 0xE0);
    __delay_cycles(2516582/100);

    /* 开启加速度与陀螺仪都工作 */
    ICM_Write_Registers(ICM20602_PWR_MGMT2_REG, 0x07); //0x00为加速度陀螺仪6轴全开启，0x07关闭陀螺仪3轴
    __delay_cycles(2516582/100);

    return 0;
}

/*-------------------------- 设置满量程范围 ---------------------*/
/* 设置GYRO陀螺仪的满量程范围 */
void ICM_Set_Gyro_Fsr(uint8_t fsr)
{
    return ICM_Write_Registers(ICM20602_GYRO_CONFIG_REG, fsr << 3);
}


/* 设置ACCEL加速度计的满量程范围 */
void ICM_Set_Accel_Fsr(uint8_t fsr)
{
    return ICM_Write_Registers(ICM20602_ACCEL_CONFIG1_REG, fsr << 3);
}

/*------------------------- 设置采样频率 -------------------------*/
/**
  * 设置ICM20602的采样频率(假定Fs=1KHz)
  * INTERNAL_SAMPLE_RATE = 1 kHz
  * SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV);
  * rate:4 ~ 1000(Hz)
  * 返回值:0,设置成功；其他,设置失败
  */
void ICM_Set_Rate(uint16_t rate)
{
    uint8_t div;

    if (rate > 1000) {
        rate = 1000;
    } else if (rate < 4) {
        rate = 4;
    }

    /* 设置预分频值DIV */
    div = 1000 / rate - 1;
    ICM_Write_Registers(ICM20602_SMPLRT_DIV, div);

    /* 配置低通滤波器为采样率的1/2 */
    ICM_Set_DLPF(rate / 2);
}


/**
  * 设置ICM20602的数字低通滤波器(假定Fs=1KHz)
  * 返回值:0,设置成功；其他,设置失败
  */
void ICM_Set_DLPF(uint16_t dlpf)
{
    uint8_t data;

    if (dlpf >= 250) {
        data = 0;
    } else if (dlpf >= 176) {
        data = 1;
    } else if (dlpf >= 92) {
        data = 2;
    } else if (dlpf >= 41) {
        data = 3;
    } else if (dlpf >= 20) {
        data = 4;
    } else if (dlpf >= 10) {
        data = 5;
    } else {
        data = 6;
    }

    ICM_Write_Registers(ICM20602_CONFIG_REG, data);
}

/* ---------------------- 读取传感器采集到的数据 -------------------------- */
// /**
//   * 读取陀螺仪值(原始值)
//   * gx,gy,gz：陀螺仪x,y,z轴的原始读数(带符号)
//   * 返回值：0,成功; 其他,错误代码
//   */
// void ICM_Get_Gyroscope(uint16_t* gx, uint16_t* gy, uint16_t* gz)
// {
//     uint8_t buf[6], res;
//     ICM_Read_Len(ICM20602_GYRO_XOUT_H, buf, 6);
//
//     *gx = ((uint16_t)buf[0] << 8) | buf[1];
//     *gy = ((uint16_t)buf[2] << 8) | buf[3];
//     *gz = ((uint16_t)buf[4] << 8) | buf[5];
// }

/**
  * 读取加速度计值(原始值)
  * ax,ay,az：加速度计x,y,z轴的原始读数(带符号)
  * 返回值：0,成功; 其他,错误代码
  */
void ICM_Get_Accelerometer(uint16_t* ax, uint16_t* ay, uint16_t* az)
{
    uint8_t buf[6];
    ICM_Read_Len(ICM20602_ACCEL_XOUT_H, buf, 6);
    *ax = ((uint16_t)buf[0] << 8) | buf[1];
    *ay = ((uint16_t)buf[2] << 8) | buf[3];
    *az = ((uint16_t)buf[4] << 8) | buf[5];
}

//******************************************************************************
//
//This is the USCI_B0 interrupt vector service routine.
//
//******************************************************************************
#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=USCI_B0_VECTOR
__interrupt
#elif defined(__GNUC__)
__attribute__((interrupt(USCI_B0_VECTOR)))
#endif
void ICM_USCI_B0_ISR (void)
{
    switch (__even_in_range(UCB0IV,12)){
        case USCI_I2C_UCTXIFG:
        {
        	if(IcmTXByteCtrTotal == -2)
        	{
                USCI_B_I2C_clearInterrupt(ICM_IIC_BASE,USCI_B_I2C_TRANSMIT_INTERRUPT);
        		USCI_B_I2C_setMode(ICM_IIC_BASE,USCI_B_I2C_RECEIVE_MODE);
        		USCI_B_I2C_masterReceiveMultiByteStart(ICM_IIC_BASE);
        	}
        	//Check TX byte counter
        	else if (IcmTXByteCtrTotal > 0)
        	{
                //Initiate send of character from Master to Sla
                USCI_B_I2C_masterSendMultiByteNext(ICM_IIC_BASE,*PIcmTxData++);
                //Increment TX byte counter
                IcmTXByteCtrTotal--;
            }
        	else
        	{
                //Initiate stop only
                USCI_B_I2C_masterSendMultiByteStop(ICM_IIC_BASE);
                //Clear master interrupt status
                USCI_B_I2C_clearInterrupt(ICM_IIC_BASE,USCI_B_I2C_TRANSMIT_INTERRUPT);
                IcmTXByteCtrTotal--;
        	}
            break;
        }
        case USCI_I2C_UCRXIFG:
        {
            IcmRXByteCtr--;
        	if (IcmRXByteCtr)
        	{
                if (IcmRXByteCtr == 1)
                {
                    //Initiate end of reception -> Receive byte with NAK
                    *PIcmRxData++ = USCI_B_I2C_masterReceiveMultiByteFinish(ICM_IIC_BASE);
                }
                else
                {
                    //Keep receiving one byte at a time
                    *PIcmRxData++ = USCI_B_I2C_masterReceiveMultiByteNext(ICM_IIC_BASE);
                }
            }
            else
            {
                //Receive last byte
	            *PIcmRxData++ = USCI_B_I2C_masterReceiveMultiByteNext(ICM_IIC_BASE);
	            break;
        	}
        }
        default:break;
    }
}
